
#include "util.h"

#include <tiros/tiros.h>

  


#define STKSIZE             (TIROS_MIN_CTXT_SZ + 64)
#define NUM_TASKS	     4



#define TST_QSZ		4
osword_t tst_Q[msgQ_MEMSZ(TST_QSZ)];
csem_t cs;



/* Stacks for each of our tasks */
osword_t task_stack[NUM_TASKS][STKSIZE];




void wait(void)
{
	delayms(1000);
}


void idle_task(void *dummy)
{
	while(1) 
		wait();
}

static tid_t T0tid, T1tid, T2tid, idle_tid;


void T0(void *dummy)
{
	int i;
	int8_t rc;
	char ch = 'A';

	while(1) {
		csem_P(&cs, 0, 0);
		rc = os_task_resume(T1tid);
		putstring("T0 resuming T1\n");
		for (i=0; i<= (TST_QSZ+2); i++) {
			putstring("T0: sending : ");
			putchar(ch); putchar('\n');
			rc = msgQ_send( (msgQ_t*) tst_Q, (osptr_t)(osptrword_t)ch);
			if (rc == SUCCESS)
				putstring("T0: success\n");
			else if(rc == ERR_FULL) 
				putstring("T0: Q full\n");
			else
				putstring("T0: unknown error\n");
			ch++;
			if (ch == 'Z')
				ch = 'A';

		}
	}
	

}

void T1(void *dummy)
{
	int8_t rc;
	osptr_t rx_val;
	char receivedchar;
	while(1) {
		rc = msgQ_recv( (msgQ_t*) tst_Q, 0, 0, &rx_val);
		if (rc == SUCCESS) {
			putstring("T1: recvd message : "); 
			receivedchar = (char) (osptrword_t)rx_val;
			putchar(receivedchar); putchar('\n');

		} else if (rc == ERR_WOULDBLOCK_ISR) {
			putstring("T1: wouldblock ISR (bad ret)\n");
		} else if (rc == ERR_WOULDBLOCK_MUTEX) {
			putstring("T1: wouldblock Mutex (bad ret)\n");
		} else if (rc == ERR_TIMEOUT) {
			putstring("T1: ERR_timeout (bad ret)\n");
		} else if (rc == ERR_RESUMED) {
			putstring("T1: ERR_Resumed\n");
		} else 
			putstring("T1: recv error\n");
			
	}
}

void T2(void *dummy)
{
	int8_t rc;
	char ch = 'a';
	while(1) {
		putstring("T2: sending : ");
		putchar(ch); putchar('\n');

		rc = msgQ_send( (msgQ_t*) tst_Q, (osptr_t)(osptrword_t)ch);
		if (rc == SUCCESS)
			putstring("T2: success\n");
		else if(rc == ERR_FULL) 
			putstring("T2: Q full\n");
		else
			putstring("T2: unknown error\n");
		
		ch++;
		if (ch == 'z')
			ch = 'a';
		wait();
		wait();
		csem_V(&cs);

	}
}


void platform_init(void);

int main(void) 
{
	platform_init();

	// --------------------------------------------------
	os_init();

	/* Initialize the message Q */
	msgQ_init( (msgQ_t*)tst_Q, TST_QSZ);


	/* Priorities as follows:
	 * T0 : 0
	 * T1 : 1
	 * T2 : 2
	 * idle: 3 */

	
	csem_init(&cs, 1, TIROS_CSEMVAL_MAX);


	T0tid = os_task_create(T0, (osptr_t)0,
			       task_stack[0], STKSIZE, 0); 

	T1tid = os_task_create(T1, (osptr_t)0,
			       task_stack[1], STKSIZE, 1); 

	T2tid = os_task_create(T2, (osptr_t)0,
			       task_stack[2], STKSIZE, 2); 

	idle_tid = os_task_create(idle_task, (osptr_t)0,
			       task_stack[3], STKSIZE, 3); 


	if (T0tid == ILLEGAL_ELEM || T1tid == ILLEGAL_ELEM ||
	    T2tid == ILLEGAL_ELEM || idle_tid == ILLEGAL_ELEM) {
			putstring("Task create failed");
	}
	os_start();
	putstring("Started OS: Shouldn't reach here\n");
	while (1);
}

